#include <stdlib.h>

#include <numeric>
#include <limits>
#include <string>
#include <sstream>
#include <random>

#ifndef _USE_MATH_DEFINES
#define _USE_MATH_DEFINES
#include <math.h>
#endif

#include "pso.h"

#include "CImg.h"
using namespace cimg_library;

extern std::vector<std::pair<double,double>>  linechart_in;
extern std::vector<std::pair<double,double>>* linechart;

extern CImg<unsigned char> img_rgb, seg_face;
extern int Dx, Dy;

//----------------------------------------------------------------------------
// PSO parameters
//----------------------------------------------------------------------------
extern int Dim;
extern int numParticles;
extern int numIterations;
extern std::vector<double> BoundPosMin;
extern std::vector<double> BoundPosMax;
extern std::vector<double> BoundVeloMin;
extern std::vector<double> BoundVeloMax;
//----------------------------------------------------------------------------


//----------------------------------------------------------------------------
// PSO constrain by region
//----------------------------------------------------------------------------
extern int numConstrains;
extern double region_factor;
extern CImg<int>* region;
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------
// PSO constrain by edge
//----------------------------------------------------------------------------

//----------------------------------------------------------------------------


//----------------------------------------------------------------------------
// PSO parameters
//----------------------------------------------------------------------------
// - All parameter values are set to values recommended b a research paper
//   that investigated the effects of various PSO parameter values on a set of
//   benchmark minimization problems.
// - Alternatively, the inertia weight can be dynamically vary according to 
//   the iterations.
// - If you set the cognitive value to be larger than the value of social weight
//   you place more weight on a particle's best known position that on the 
//   swarm's lobal best known position, and vice versa.
//----------------------------------------------------------------------------
extern double  w;    // inertia weight
extern double c1;  // cognitive weight
extern double c2;  // social weight
extern double c3;  // global weight
// ----------------------------------------------------------------------------






typedef std::vector<PSO::Swarm*> SuperSwarm;

static void init_superswarm(SuperSwarm& superswarm, Utils::Random& ran)
{
	const int nSubBound = 2;

	double rangex = BoundPosMax[2] - BoundPosMin[3];
	double rangey = BoundPosMax[3] - BoundPosMin[3];
	double deltax = rangex / (double)nSubBound;
	double deltay = rangey / (double)nSubBound;

	std::vector<double> subBoundPosMax(Dim);
	std::vector<double> subBoundPosMin(Dim);
	std::vector<double> subBoundVeloMax(Dim);
	std::vector<double> subBoundVeloMin(Dim);

	for (int i=0; i<Dim; i++) {
		subBoundPosMax[i] = BoundPosMax[i];
		subBoundPosMin[i] = BoundPosMin[i];
		subBoundVeloMin[i] = BoundVeloMin[i];
		subBoundVeloMax[i] = BoundVeloMax[i];
	}

	// Create sub-swarm for scales
	for (int suby=0; suby<nSubBound; suby++) {
		double subminy = BoundPosMin[3] + suby*deltay;
		double submaxy = subminy + deltay;
		
		for (int subx=0; subx<nSubBound; subx++) {
			double subminx = BoundPosMin[2] + subx*deltax;
			double submaxx = subminx + deltax;

			subBoundPosMin[2]  = subminx;
			subBoundPosMax[2]  = submaxx;
			subBoundVeloMin[2] = -submaxx;
			subBoundVeloMax[2] = submaxx;

			subBoundPosMin[3]  = subminy;
			subBoundPosMax[3]  = submaxy;
			subBoundVeloMin[3] = -submaxy;
			subBoundVeloMax[3] = submaxy;

			superswarm.push_back( 
				new PSO::Swarm( numParticles,
				subBoundPosMin, subBoundPosMax, 
				subBoundVeloMin, subBoundVeloMax,
				ran));
		}
	}

	for (unsigned int i=0; i <superswarm.size(); i++) {
		superswarm[i]->set_weights(w,c1,c2,c3);
	}




}

static void update_superswarm(
	SuperSwarm& superswarm, 
	std::vector<double>& superGlobalPosition,
	double& superGlobalFitness) {

		for (unsigned int i=0; i<superswarm.size(); i++) {
			PSO::Swarm* swarm = superswarm[i];
			if (superGlobalFitness < swarm->get_best_fitness()) {
				superGlobalFitness = swarm->get_best_fitness();
				swarm->copy_best_position_to(superGlobalPosition);
			}
		}

		for (unsigned int i=0; i<superswarm.size(); i++) {
			PSO::Swarm* swarm = superswarm[i];
			swarm->set_super_fitness(superGlobalFitness);
			swarm->copy_super_position_from(superGlobalPosition);
		}
}

void app_multi_pso(bool show)
{
	Utils::Random ran;

	CImg<unsigned char> canvas;
	CImgDisplay disp(cimg_fitscreen(Dx,Dy,1),"Particle Swarm Optimization");
	const char red[] = {255,0,0}, blue[] = {0,0,255}, black[] = {0,0,0};	

	if (show) {
		canvas.assign(img_rgb);
		cimg_forXYC(canvas,x,y,c) {
			if ((*region)(x,y)==1) continue;
			canvas(x,y,c) = canvas(x,y,c)*0.7f + seg_face(x,y,c)*0.3f;
		}
		canvas.display(disp);
	}

	SuperSwarm superswarm;
	std::vector<double> superGlobalPosition(Dim);
	double superGlobalFitness = DBL_MIN;

	init_superswarm(superswarm, ran);
	update_superswarm(superswarm, superGlobalPosition, superGlobalFitness);

	if (show) {
		double tx,ty,sx,sy,a,b;
		for (unsigned int i=0; i<superswarm.size(); i++) {
			tx = superswarm[i]->get_best_position(0);
			ty = superswarm[i]->get_best_position(1);
			sx = superswarm[i]->get_best_position(2);
			sy = superswarm[i]->get_best_position(3);

			tx *= sx;
			ty *= sy;

			if ((int)tx<0 || (int)tx>=img_rgb.width() || (int)ty<0 || (int)ty>=img_rgb.height())
				continue;

			a = (6.0*sx < 1.0) ? 1.0: 6.0*sx;
			b = (6.0*sy < 1.0) ? 1.0: 6.0*sy;
			canvas.draw_ellipse((int)tx, (int)ty, a, b, 0, red);
		}
		
		tx = superGlobalPosition[0];
		ty = superGlobalPosition[1];
		sx = superGlobalPosition[2];
		sy = superGlobalPosition[3];

		tx *= sx; 
		ty *= sy;

		if (!((int)tx<0 || (int)tx>=img_rgb.width() || (int)ty<0 || (int)ty>=img_rgb.height())) {
			a = (6.0*sx < 1.0) ? 1.0: 6.0*sx;
			b = (6.0*sy < 1.0) ? 1.0: 6.0*sy;			
			canvas.draw_ellipse((int)tx, (int)ty, a, b, 0, blue);
		}
		canvas.display(disp);
	}


	for (int iter=0; iter < numIterations; iter++) {
		for (unsigned int s=0; s<superswarm.size(); s++) {
			superswarm[s]->Run(20, ran, region, &seg_face, &linechart_in, &img_rgb);

			//if (superswarm[s]->get_best_fitness() > superGlobalFitness) {
			//	superGlobalFitness = superswarm[s]->get_best_fitness();
			//	superswarm[s]->copy_best_position_to(superGlobalPosition);
			//	update_superswarm(superswarm,superGlobalPosition,superGlobalFitness);
			//}
		}
		update_superswarm(superswarm,superGlobalPosition,superGlobalFitness);
		

		if (show) {
			canvas.assign(img_rgb);
			cimg_forXYC(canvas,x,y,c) {
				if ((*region)(x,y)==1) continue;
				canvas(x,y,c) = canvas(x,y,c)*0.7f + seg_face(x,y,c)*0.3f;
			}

			double tx,ty,sx,sy,a,b;
			for (unsigned int i=0; i<superswarm.size(); i++) {
				tx = superswarm[i]->get_best_position(0);
				ty = superswarm[i]->get_best_position(1);
				sx = superswarm[i]->get_best_position(2);
				sy = superswarm[i]->get_best_position(3);

				tx *= sx;
				ty *= sy;

				if ((int)tx<0 || (int)tx>=img_rgb.width() || (int)ty<0 || (int)ty>=img_rgb.height())
					continue;

				a = (6.0*sx < 1.0) ? 1.0: 6.0*sx;
				b = (6.0*sy < 1.0) ? 1.0: 6.0*sy;
				canvas.draw_ellipse((int)tx, (int)ty, a, b, 0, red);
			}

			tx = superGlobalPosition[0];
			ty = superGlobalPosition[1];
			sx = superGlobalPosition[2];
			sy = superGlobalPosition[3];

			tx *= sx;
			ty *= sy;

			if (!((int)tx<0 || (int)tx>=img_rgb.width() || (int)ty<0 || (int)ty>=img_rgb.height())) {
				a = (6.0*sx < 1.0) ? 1.0: 6.0*sx;
				b = (6.0*sy < 1.0) ? 1.0: 6.0*sy;			
				canvas.draw_ellipse((int)tx, (int)ty, a, b, 0, blue);
			}

			tx = superGlobalPosition[0];
			ty = superGlobalPosition[1];
			double x0 = (linechart_in[0].first + tx)*sx;
			double y0 = (linechart_in[0].second + ty)*sy;
			for (unsigned int i=1; i<linechart_in.size(); i++) {
				double x1 = (linechart_in[i].first + tx)*sx;
				double y1 = (linechart_in[i].second + ty)*sx;

				if ((int)x0<0 || (int)x0>=img_rgb.width() || (int)y0<0 || (int)y0>img_rgb.height() ||
					(int)x1<0 || (int)x1>=img_rgb.width() || (int)y1<0 || (int)y1>img_rgb.height()) {
						;
				} else {
					canvas.draw_line((int)x0,(int)y0,(int)x1,(int)y1,blue);
				}
				x0 = x1; y0 = y1;
			}

			std::stringstream ss;
			ss << iter << " / " << numIterations;
			canvas.draw_text(20,20,ss.str().c_str(),black,0);

			ss.str(std::string()); 
			ss << "Best Fit: " << superGlobalFitness;
			canvas.draw_text(20,40,ss.str().c_str(),black,0);

			ss.str(std::string());
			ss << "Best Sol: " << superGlobalPosition[0] << " , " << superGlobalPosition[1] << " , "
			                   << superGlobalPosition[2] << " , " << superGlobalPosition[3];
			canvas.draw_text(20,60,ss.str().c_str(),black,0);

			canvas.display(disp);
			disp.wait(100);
		}
	}

	for (unsigned int i=0; i<superswarm.size(); i++)
		delete superswarm[i];

	if (show) {
		while (!disp.is_closed());
	}
}


























